package dev.ece.util.lib;

import javax.vecmath.Quat4f;

import org.lwjgl.opengl.GL20;

import com.bulletphysics.linearmath.QuaternionUtil;

/**
 * @author 潜行的青衣
 * @see
 */
public class Vector4f extends Vector3f {

	public Vector4f() {
		super(new float[4]);
	}
	
	public Vector4f(float x, float y, float z, float w) {
		super(new float[] {x, y, z, w});
	}
	
	public Vector4f(Vector4f vector) {
		super(new float[] {vector.getX(), vector.getY(), vector.getZ(), vector.getW()});
	}
	
	public Vector4f(Vector3f vec, float angle) {
		super(new float[] {vec.getX(), vec.getY(), vec.getZ(), angle});
	}
	
	public void set(Vector4f vector) {
		this.set(vector.getX(), vector.getY(), vector.getZ(), vector.getW());
	}
	
	public void set(float x, float y, float z, float w) {
		this.setX(x);
		this.setY(y);
		this.setZ(z);
		this.setW(w);
	}
	
	public Quat4f toQuat4f() {
		Quat4f result = new Quat4f();
		QuaternionUtil.setRotation(result, this.toVector3f(), (float) Math.toRadians(this.getAngle()));
		return result;
	}
	
	public final Vector4f fromEulerToQuaternion(Vector3f rotate) {
		return fromEulerToQuaternion(rotate.getX(), rotate.getY(), rotate.getZ());
	}
	
	/**
	 * 欧拉角转四元数
	 * 设置
	 * @param x 沿x轴旋转角度
	 * @param y 沿y轴旋转角度
	 * @param z 沿z轴旋转角度
	 */
	public final Vector4f fromEulerToQuaternion(float x, float y, float z) {
		float angle = 0.0f;
        float sr, sp, sy, cr, cp, cy;
        //rescale the inputs to 1/2 angle
        angle = z * 0.5f;
        sy = (float) Math.sin(angle);
        cy = (float) Math.cos(angle);
        angle = y * 0.5f;
        sp = (float) Math.sin(angle);
        cp = (float) Math.cos(angle);
        angle = x * 0.5f;
        sr = (float) Math.sin(angle);
        cr = (float) Math.cos(angle);

        this.setX(sr * cp * cy - cr * sp * sy); // X
        this.setY(cr * sp * cy + sr * cp * sy); // Y
        this.setZ(cr * cp * sy - sr * sp * cy); // Z
        this.setW(cr * cp * cy + sr * sp * sy); // W
        return this;
	}
	
	/**
	 * 摘自网上算法
	 * 插值运算
	 * @param v1
	 * @param v2
	 * @param alpha
	 */
	public final void interpolate(Vector4f v1, Vector4f v2, float alpha) {
		float cosa = v1.getX() * v2.getX() + v1.getY() * v2.getY() + 
				v1.getZ() * v2.getZ() + v1.getW() * v2.getW();
		Vector4f ending = null;
	    if (cosa < 0.0f ) {
	    	ending = new Vector4f(-v2.getX(), -v2.getY(), -v2.getZ(), -v2.getW());
	        cosa = -cosa;
	    } else {
	    	ending = v2;
	    }
	    float k0, k1;
	    if(cosa > 0.9999f) {
	        k0 = 1.0f - alpha;
	        k1 = alpha;
	    } else {
	        float sina = (float) Math.sqrt(1.0f - cosa * cosa);
	        float a = (float) Math.atan2(sina, cosa);
	        float invSina = 1.0f / sina;
	        k0 = (float) Math.sin((1.0f - alpha)*a) * invSina;
	        k1 = (float) Math.sin(alpha * a) * invSina;
	    }
	    this.setX(v1.getX() * k0 + ending.getX() * k1);
	    this.setY(v1.getY() * k0 + ending.getY() * k1);
	    this.setZ(v1.getZ() * k0 + ending.getZ() * k1);
	    this.setW(v1.getW() * k0 + ending.getW() * k1);
    }
	
	public void glUniform4f(int location) {
		GL20.glUniform4f(location, this.getX(), this.getY(), this.getZ(), this.getW());
	}
	
	public void setRed(float red) {
		this.setX(red);
	}
	
	public float getRed() {
		return getX();
	}
	
	public void setGreen(float green) {
		this.setY(green);
	}
	
	public float getGreen() {
		return this.getY();
	}
	
	public void setBlue(float blue) {
		this.setZ(blue);
	}
	
	public float getBlue() {
		return this.getZ();
	}
	
	public void setW(float w) {
		this.getVector()[3] = w;
	}
	
	public float getW() {
		return getVector()[3];
	}
	
	public void setAlpha(float alpha) {
		this.setW(alpha);
	}
	
	public float getAlpha() {
		return this.getW();
	}
	
	public void setAngle(float angle) {
		this.setW(angle);
	}
	
	public float getAngle() {
		return getW();
	}

	@Override
	public boolean equals(Object obj) {
		if(obj == null) {
			return false;
		}
		if(!(obj instanceof Vector4f)) {
			return false;
		}
		Vector4f v = (Vector4f) obj;
		return v.getX() == this.getX() 
				&& v.getY() == this.getY() 
				&& v.getZ() == this.getZ()
				&& v.getW() == this.getW();
	}
}
